//
// Created by cyt on 2023/9/4.
//
#include "icm42688_base.h"
#include "math.h"

IcmRawData_t Icm42688::icm_data;
InvnAlgoAGMInput Icm42688::input;
InvnAlgoAGMOutput Icm42688::output;
clk_calib_t Icm42688::clk_calib;
int Icm42688::iter_algo;
struct inv_icm426xx Icm42688::icm_driver;

/**
 * @brief 通信配置初始化
 * 
 * @param read_reg 通信读取函数指针
 * @param write_reg 通信发送函数指针
 * @param max_read  最大读取长度
 * @param max_write 最大发送长度
 */


void Icm42688::ConfigInit(int (*read_reg)(struct inv_icm426xx_serif *, uint8_t, uint8_t *,
										  uint32_t),
						  int (*write_reg)(struct inv_icm426xx_serif *, uint8_t, const uint8_t *,
										   uint32_t),
						  uint32_t max_read,
						  uint16_t max_write)
{
	/* Initialize serial inteface between MCU and Icm426xx */
	icm_serif.context = 0; /* no need */
	icm_serif.read_reg = read_reg;
	icm_serif.write_reg = write_reg;
	icm_serif.max_read = max_read;	 /* maximum number of bytes allowed per serial read */
	icm_serif.max_write = max_write; /* maximum number of bytes allowed per serial write */
	icm_serif.serif_type = SERIF_TYPE;
}


/**
 * @brief IMU参数配置
 * 
 * @param is_low_noise_mode 
 * @param acc_fsr_g 
 * @param gyr_fsr_dps 
 * @param acc_freq 
 * @param gyr_freq 
 * @param is_rtc_mode 
 * @return int 
 */
int Icm42688::ConfigureInvDevice(uint8_t is_low_noise_mode, int32_t acc_fsr_g,
								 int32_t gyr_fsr_dps,
								 ICM426XX_ACCEL_CONFIG0_ODR_t acc_freq, ICM426XX_GYRO_CONFIG0_ODR_t gyr_freq,
								 uint8_t is_rtc_mode)
{
	int rc = 0;

	// rc |= inv_flash_manager_init();

#if USE_CLK_IN
#if defined(ICM42633)
	/*
	 * ICM42633 is a triple interface device. To access CLKIN, AUX2 interface needs to be disabled.
	 * Use INV_ICM426XX_DUAL_INTERFACE mode. The following mode are also compatible:
	 *  - INV_ICM426XX_SINGLE_INTERFACE
	 *  - INV_ICM426XX_DUAL_INTERFACE_SPI4
	 */
	rc |= inv_icm426xx_interface_change_procedure(&icm_driver, INV_ICM426XX_DUAL_INTERFACE);
#endif
	/* Enable CLKIN */
	if (inv_icm426xx_enable_clkin_rtc(&icm_driver, 1) < 0)
	{
		INV_MSG(INV_MSG_LEVEL_ERROR, "Couldn't enable CLKIN");
		return -1;
	}
	clock_calibration_reset(&icm_driver, &clk_calib);
	/* Ensure all coefficients are set to 1 as the clock will not drift */
	clk_calib.coef[INV_ICM426XX_PLL] = 1.0f;
	clk_calib.coef[INV_ICM426XX_RC_OSC] = 1.0f;
	clk_calib.coef[INV_ICM426XX_WU_OSC] = 1.0f;
#else
	rc |= clock_calibration_init(&icm_driver, &clk_calib);
#endif

#if IS_HIGH_RES_MODE
	rc |= inv_icm426xx_enable_high_resolution_fifo(&icm_driver);
#else
	rc |= inv_icm426xx_set_accel_fsr(
		&icm_driver, (ICM426XX_ACCEL_CONFIG0_FS_SEL_t)accel_fsr_g_to_bitfield(acc_fsr_g));

	acc_fsr = acc_fsr_g;
	gyr_fsr = gyr_fsr_dps;
	set_acc_sensitivity();
	set_gyro_snesitivity();

	rc |= inv_icm426xx_set_gyro_fsr(
		&icm_driver, (ICM426XX_GYRO_CONFIG0_FS_SEL_t)gyro_fsr_dps_to_bitfield(gyr_fsr_dps));
#endif

	rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ACCEL_FREQ);
	rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, GYRO_FREQ);

#if IS_LOW_NOISE_MODE
	rc |= inv_icm426xx_enable_accel_low_noise_mode(&icm_driver);
#else
	rc |= inv_icm426xx_enable_accel_low_power_mode(&icm_driver);
#endif

	rc |= inv_icm426xx_enable_gyro_low_noise_mode(&icm_driver);

	return rc;
}


/**
 * @brief 初始化并启动IMU
 * 
 */
int Icm42688::SetupInvDevice(struct inv_icm426xx_serif *icm_serif, uint8_t _enable_fifo)
{
	int rc = 0;
	uint8_t who_am_i;

	/* Initialize device */
	INV_MSG(INV_MSG_LEVEL_INFO, "Initialize Icm426xx");

	rc = inv_icm426xx_init(&icm_driver, icm_serif);
	 if(_enable_fifo)
		icm_driver.sensor_event_cb = HandleInvDeviceFifoPacket;
	 else
        icm_driver.sensor_event_cb = HandleInvDeviceDataRegisters;
	if (rc != INV_ERROR_SUCCESS)
	{
		INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR : failed to initialize Icm426xx.");
		return rc;
	}

	/* Check WHOAMI */
	INV_MSG(INV_MSG_LEVEL_INFO, "Check Icm426xx whoami value");

	rc = inv_icm426xx_get_who_am_i(&icm_driver, &who_am_i);
	if (rc != INV_ERROR_SUCCESS)
	{
		INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR : failed to read Icm426xx whoami value.");
		return rc;
	}

	if (who_am_i != ICM_WHOAMI)
	{
		INV_MSG(INV_MSG_LEVEL_ERROR, "!!! ERROR :  bad WHOAMI value. Got 0x%02x (expected: 0x%02x)",
				who_am_i, ICM_WHOAMI);
		return INV_ERROR;
	}

	RINGBUFFER_CLEAR(&timestamp_buffer);
	return rc;
}

/**
 * @brief 陀螺仪自检
 * 
 * @return int 
 */
int Icm42688::RunSelfTest()
{
	int rc = 0, st_result = 0;

	rc = inv_icm426xx_run_selftest(&icm_driver, &st_result);

	if (rc < 0)
	{
		INV_MSG(INV_MSG_LEVEL_ERROR, "An error occured while running selftest");
	}
	else
	{
		/* Check for GYR success (1 << 0) and ACC success (1 << 1) */
		if (st_result & 0x1)
			INV_MSG(INV_MSG_LEVEL_INFO, "Gyro Selftest PASS");
		else
			INV_MSG(INV_MSG_LEVEL_INFO, "Gyro Selftest FAIL");

		if (st_result & 0x2)
			INV_MSG(INV_MSG_LEVEL_INFO, "Accel Selftest PASS");
		else
			INV_MSG(INV_MSG_LEVEL_INFO, "Accel Selftest FAIL");
	}
	return rc;
}

/**
 * @brief 读取偏置
 * 
 */
void Icm42688::GetBias()
{
	int rc = 0;
	int raw_bias[6];

	/* Get Low Noise / Low Power bias computed by self-tests scaled by 2^16 */
	INV_MSG(INV_MSG_LEVEL_INFO, "Getting ST bias");
	rc |= inv_icm426xx_get_st_bias(&icm_driver, raw_bias);
	if (rc < 0)
		INV_MSG(INV_MSG_LEVEL_ERROR, "An error occured while getting ST bias");

	gyr_bias[0] = (float)(raw_bias[0]) / (float)(1 << 16);
	gyr_bias[1] = (float)(raw_bias[1]) / (float)(1 << 16);
	gyr_bias[2] = (float)(raw_bias[2]) / (float)(1 << 16);

	acc_bias[0] = (float)(raw_bias[0 + 3] / (float)(1 << 16));
	acc_bias[1] = (float)(raw_bias[1 + 3] / (float)(1 << 16));
	acc_bias[2] = (float)(raw_bias[2 + 3] / (float)(1 << 16));

	INV_MSG(INV_MSG_LEVEL_INFO, "GYR LN bias (dps): x=%f, y=%f, z=%f",
			bias.gyr_x, bias.gyr_y, bias.gyr_z);
	INV_MSG(INV_MSG_LEVEL_INFO, "ACC LN bias (g): x=%f, y=%f, z=%f",
			bias.acc_x, bias.acc_y, bias.acc_z);
}

/*************************************获取数据函数部分********************************************************************/
//从寄存器获取数据
int Icm42688::GetDataFromInvDevice(void)
{
	/*
	 * Read data from registers. Callback defined at init time (i.e.
	 * HandleInvDeviceDataRegisters) will be called for each valid packet extracted from
	 * FIFO.
	 */
	return inv_icm426xx_get_data_from_registers(&icm_driver);
}

int Icm42688::GetDataFromInvDeviceFIFO()
{
	/*
	 * Extract packets from FIFO. Callback defined at init time (i.e.
	 * HandleInvDeviceFifoPacket) will be called for each valid packet extracted from
	 * FIFO.
	 */
	return inv_icm426xx_get_data_from_fifo(&icm_driver);
}

/*************************************姿态数据解算部分*******************************************************/
/*
 * This function initializes biases and accuracies for accelerometer, gyroscope and magnetometer.
 */
int Icm42688::InitInvAGMBiases(void)
{
	/* Retrieve stored biases */
	if (retrieve_stored_biases_from_flash(acc_bias, gyr_bias, mag_bias) == 0) {
		INV_MSG(INV_MSG_LEVEL_INFO, "   Biases loaded from flash:");
		INV_MSG(INV_MSG_LEVEL_INFO, "    - Accel: [%f %f %f]g", (float)acc_bias[0] / (1 << 16),
				(float)acc_bias[1] / (1 << 16), (float)acc_bias[2] / (1 << 16));
		INV_MSG(INV_MSG_LEVEL_INFO, "    - Gyro:  [%f %f %f]dps", (float)gyr_bias[0] / (1 << 16),
				(float)gyr_bias[1] / (1 << 16), (float)gyr_bias[2] / (1 << 16));
		INV_MSG(INV_MSG_LEVEL_INFO, "    - Mag:   [%f %f %f]uT", (float)mag_bias[0] / (1 << 16),
				(float)mag_bias[1] / (1 << 16), (float)mag_bias[2] / (1 << 16));

		acc_accuracy = 3;
		gyr_accuracy = 2;
		mag_accuracy = 1;
	} else 
	INV_MSG(INV_MSG_LEVEL_INFO, "   No bias values retrieved");
	memset(acc_bias, 0, sizeof(acc_bias));
	memset(gyr_bias, 0, sizeof(gyr_bias));
	memset(mag_bias, 0, sizeof(mag_bias));
	acc_accuracy = 0;
	gyr_accuracy = 0;
	mag_accuracy = 0;

	return 0;
}

/*
 * This function clears biases and accuracies.
 */
int Icm42688::ResetInvAGMBiases(void)
{
	memset(acc_bias, 0, sizeof(acc_bias));
	memset(gyr_bias, 0, sizeof(gyr_bias));
	memset(mag_bias, 0, sizeof(mag_bias));
	acc_accuracy = 0;
	gyr_accuracy = 0;
	mag_accuracy = 0;

	return 0;
}

int Icm42688::InitInvAGMAlgo(void)
{
	int rc = 0;
	InvnAlgoAGMConfig config;

	memset(&input, 0, sizeof(input));
	memset(&output, 0, sizeof(output));
	memset(&config, 0, sizeof(config));

	/* FSR configurations */
	config.acc_fsr = acc_fsr;
	config.gyr_fsr = gyr_fsr;

	config.acc_odr_us = odr_bitfield_to_us(ACCEL_FREQ);
	config.gyr_odr_us = odr_bitfield_to_us(GYRO_FREQ);

	/* Temoperature sensor configuration */
 #if IS_HIGH_RES_MODE
	config.temp_sensitivity = (int32_t)(((int64_t)100 << 30) / 13248); // high-res;
	config.temp_offset = 25 << 16;
 #else
	config.temp_sensitivity = (int32_t)(((int64_t)100 << 30) / 207);
	config.temp_offset = 25 << 16;
 #endif

 #if USE_MAG
	config.mag_sc_q16 = 9830; // 0.15
	config.mag_odr_us = MAG_ODR_US;
 #else
	config.mag_bias_q16 = NULL;
 #endif

	config.acc_bias_q16 = acc_bias;
	config.gyr_bias_q16 = gyr_bias;
	config.mag_bias_q16 = mag_bias;
	config.acc_accuracy = (int8_t)acc_accuracy;
	config.gyr_accuracy = (int8_t)gyr_accuracy;
	config.mag_accuracy = (int8_t)mag_accuracy;

	/* Initialize algorithm */
	rc |= invn_algo_agm_init(&config);

	return rc;
}

/**
 * @brief 四元数转换为欧拉角
 * 
 * @param quat 
 */
void Icm42688::quaternions_to_angles(const float quat[4])
{
	const float RAD_2_DEG = (180.f / 3.14159265358979f);
	float rot_matrix[9];

	// quaternion_to_rotation_matrix
	const float dTx = 2.0f * quat[1];
	const float dTy = 2.0f * quat[2];
	const float dTz = 2.0f * quat[3];
	const float dTwx = dTx * quat[0];
	const float dTwy = dTy * quat[0];
	const float dTwz = dTz * quat[0];
	const float dTxx = dTx * quat[1];
	const float dTxy = dTy * quat[1];
	const float dTxz = dTz * quat[1];
	const float dTyy = dTy * quat[2];
	const float dTyz = dTz * quat[2];
	const float dTzz = dTz * quat[3];

	rot_matrix[0] = 1.0f - (dTyy + dTzz);
	rot_matrix[1] = dTxy - dTwz;
	rot_matrix[2] = dTxz + dTwy;
	rot_matrix[3] = dTxy + dTwz;
	rot_matrix[4] = 1.0f - (dTxx + dTzz);
	rot_matrix[5] = dTyz - dTwx;
	rot_matrix[6] = dTxz - dTwy;
	rot_matrix[7] = dTyz + dTwx;
	rot_matrix[8] = 1.0f - (dTxx + dTyy);

	eulerAngle.yaw = atan2f(-rot_matrix[3], rot_matrix[0]) * RAD_2_DEG;
	eulerAngle.roll = atan2f(-rot_matrix[7], rot_matrix[8]) * RAD_2_DEG;
	eulerAngle.pitch = asinf(-rot_matrix[6]) * RAD_2_DEG;

	//if (eulerAngle.pitch < 0.f)
		//eulerAngle.pitch += 360.f;
}

/**
 * @brief 把四元数转换为浮点数类型
 * 
 */
void Icm42688::fixedpoint_to_float(const int32_t *in, float *out, const uint8_t fxp_shift,
								   const uint8_t dim)
{
	float scale = 1.f / (float)(1 << fxp_shift);

	for (int i = 0; i < dim; i++)
		out[i] = scale * (float)in[i];
}


/*************************************回调函数部分*****************************************************************************/
/**
 * @brief 处理从寄存器读取数据的回调函数
 * 
 * @param event 
 */
void Icm42688::HandleInvDeviceDataRegisters(inv_icm426xx_sensor_event_t *event)
{
	uint64_t irq_timestamp = 0;

	/*
	 * Extract the timestamp that was buffered when current packet IRQ fired. See
	 * ext_interrupt_cb() in main.c for more details.
	 * As timestamp buffer is filled in interrupt handler, we should pop it with
	 * interrupts disabled to avoid any concurrent access.
	 */
	inv_disable_irq();
	if (!RINGBUFFER_EMPTY(&timestamp_buffer))
		RINGBUFFER_POP(&timestamp_buffer, &irq_timestamp);
	inv_enable_irq();

	apply_mounting_matrix(icm_mounting_matrix, event->accel);
	apply_mounting_matrix(icm_mounting_matrix, event->gyro);

	icm_data.gyro[0] = event->gyro[0];
	icm_data.gyro[1] = event->gyro[1];
	icm_data.gyro[2] = event->gyro[2];

	icm_data.accel[0] = event->accel[0];
	icm_data.accel[1] = event->accel[1];
	icm_data.accel[2] = event->accel[2];
}

/**
 * @brief 处理放入FIFO数据回调函数
 * 
 * @param event 
 */
 void Icm42688::HandleInvDeviceFifoPacket(inv_icm426xx_sensor_event_t *event)
{
	uint64_t irq_timestamp = 0;
	uint64_t extended_timestamp;

	/*
	 * First, let's process the sensor event timestamp.
	 * Extract the timestamp that was buffered when current packet IRQ fired. See
	 * ext_interrupt_cb() in main.c for more details.
	 * As timestamp buffer is filled in interrupt handler, we should pop it with
	 * interrupts disabled to avoid any concurrent access.
	 */
	inv_disable_irq();
	if (!RINGBUFFER_VOLATILE_EMPTY(&timestamp_buffer))
		RINGBUFFER_VOLATILE_POP(&timestamp_buffer, &irq_timestamp);
	inv_enable_irq();

	/*
	 * Extend the 16-bit timestamp from the FIFO to get an accurate timestamping
	 */
	inv_helper_extend_timestamp_from_fifo(&icm_driver, &clk_calib, event->timestamp_fsync,
	                                      irq_timestamp, event->sensor_mask, &extended_timestamp);

	input.mask = 0;

	/*
	 * Retrieve accel and gyro data
	 */
	if (event->sensor_mask & (1 << INV_ICM426XX_SENSOR_ACCEL)) {
 #if IS_HIGH_RES_MODE
		input.sRacc_data[0] = (((int32_t)event->accel[0] << 4)) | event->accel_high_res[0];
		input.sRacc_data[1] = (((int32_t)event->accel[1] << 4)) | event->accel_high_res[1];
		input.sRacc_data[2] = (((int32_t)event->accel[2] << 4)) | event->accel_high_res[2];
 #else
		input.sRacc_data[0] = (int32_t)event->accel[0] << 4;
		input.sRacc_data[1] = (int32_t)event->accel[1] << 4;
		input.sRacc_data[2] = (int32_t)event->accel[2] << 4;
 #endif
		apply_mounting_matrix_32(icm_mounting_matrix, input.sRacc_data);
		input.mask |= INVN_ALGO_AGM_INPUT_MASK_ACC;
	}

	if (event->sensor_mask & (1 << INV_ICM426XX_SENSOR_GYRO)) {
 #if IS_HIGH_RES_MODE
		input.sRgyr_data[0] = (((int32_t)event->gyro[0] << 4)) | event->gyro_high_res[0];
		input.sRgyr_data[1] = (((int32_t)event->gyro[1] << 4)) | event->gyro_high_res[1];
		input.sRgyr_data[2] = (((int32_t)event->gyro[2] << 4)) | event->gyro_high_res[2];
 #else
		input.sRgyr_data[0] = (int32_t)event->gyro[0] << 4;
		input.sRgyr_data[1] = (int32_t)event->gyro[1] << 4;
		input.sRgyr_data[2] = (int32_t)event->gyro[2] << 4;
 #endif
		apply_mounting_matrix_32(icm_mounting_matrix, input.sRgyr_data);
		input.mask |= INVN_ALGO_AGM_INPUT_MASK_GYR;
	}

	input.sRtemp_data   = event->temperature;
	input.sRimu_time_us = extended_timestamp;

	/* Process the AgmFusion Algo */
	invn_algo_agm_process(&input, &output);

	store_biases();

	/* Print data based on the gyro rate */
	/*if (output.mask & INVN_ALGO_AGM_OUTPUT_MASK_GYRO_CAL) {
		iter_algo++;

		if (iter_algo % ((int)(print_period_us / odr_bitfield_to_us(GYRO_FREQ))) == 0)
			print_algo_inputs_outputs();
	}*/
}
/******************************Flash处理函数**************************************************************/
void Icm42688::store_biases(void)
{
	static sensor_biases_t acc_bias;
	static sensor_biases_t gyr_bias;
	static sensor_biases_t mag_bias;
	static uint8_t         biases_stored = 0;

	if (!biases_stored) {
		if (output.acc_accuracy_flag == 3) {
			memcpy(acc_bias.bias_q16, output.acc_bias_q16, sizeof(output.acc_bias_q16));
			acc_bias.is_saved = 1;
		}

		if (output.gyr_accuracy_flag == 3) {
			memcpy(gyr_bias.bias_q16, output.gyr_bias_q16, sizeof(output.gyr_bias_q16));
			gyr_bias.is_saved = 1;
		}

 #if USE_MAG
		if (mag_init_successful) {
			if (output.mag_accuracy_flag == 3) {
				memcpy(mag_bias.bias_q16, output.mag_bias_q16, sizeof(output.mag_bias_q16));
				mag_bias.is_saved = 1;
			}
		} else {
			/* Mag is not connected so let's put zeros */
			memset(mag_bias.bias_q16, 0, sizeof(mag_bias.bias_q16));
			mag_bias.is_saved = 1;
		}
 #else
		/* Mag is not connected so let's put zeros */
		memset(mag_bias.bias_q16, 0, sizeof(mag_bias.bias_q16));
		mag_bias.is_saved = 1;
 #endif

		if ((acc_bias.is_saved == 1) && (gyr_bias.is_saved == 1) && (mag_bias.is_saved == 1)) {
			/*
			 * WARNING: With this configuration, the bias can be stored up to 186 iterations in flash before erase sector.
			 * The erase procedure with the first write, can take up to 250ms.
			 * In this example, the erase is done dynamicly. Depending on the context, it could be better to do it
			 * when the software go to shutdown.
			 */
			store_biases_in_flash(acc_bias.bias_q16, gyr_bias.bias_q16, mag_bias.bias_q16);
			biases_stored = 1;
		}
	}
}

void Icm42688::store_biases_in_flash(const int32_t acc_bias_q16[3], const int32_t gyr_bias_q16[3],
						   const int32_t mag_bias_q16[3])
{
	uint8_t sensors_biases[84] = {0};
	uint8_t idx = 0;

	memcpy(&sensors_biases[idx], acc_bias_q16, sizeof(acc_bias_q16[0]) * 3);
	idx += sizeof(acc_bias_q16[0]) * 3;

	memcpy(&sensors_biases[idx], gyr_bias_q16, sizeof(gyr_bias_q16[0]) * 3);
	idx += sizeof(gyr_bias_q16[0]) * 3;

	memcpy(&sensors_biases[idx], mag_bias_q16, sizeof(mag_bias_q16[0]) * 3);

//  inv_flash_manager_writeData(sensors_biases);
}

/*
 * \brief Read NV memory and retrieve stored biases
 * \param[out] acc_bias_q16 Previously stored acc bias
 * \param[out] gyr_bias_q16 Previously stored gyr bias
 * \param[out] mag_bias_q16 Previously stored mag bias
 * \return 0 on success, -1 if no bias are in NV, an error otherwise
 */
int Icm42688::retrieve_stored_biases_from_flash(int32_t acc_bias_q16[3], int32_t gyr_bias_q16[3],
                                             int32_t mag_bias_q16[3])
{
	uint8_t sensor_bias[84];
	uint8_t idx = 0;
	int     rc;

	/* Retrieve bias stored in NV memory */
//	if ((rc = inv_flash_manager_readData(sensor_bias)) != 0) {
//		return -1;
//	}

//	memcpy(acc_bias_q16, &sensor_bias[idx], sizeof(acc_bias_q16[0]) * 3);
//	idx += sizeof(acc_bias_q16[0]) * 3;

//	memcpy(gyr_bias_q16, &sensor_bias[idx], sizeof(gyr_bias_q16[0]) * 3);
//	idx += sizeof(gyr_bias_q16[0]) * 3;

//	memcpy(mag_bias_q16, &sensor_bias[idx], sizeof(mag_bias_q16[0]) * 3);

	return rc;
}


/******************************数据处理辅助函数***************************************************/
void Icm42688::apply_mounting_matrix(const int32_t matrix[9], int16_t raw[3])
{
	unsigned i;
	int64_t data_q30[3];

	for (i = 0; i < 3; i++)
	{
		data_q30[i] = ((int64_t)matrix[3 * i + 0] * raw[0]);
		data_q30[i] += ((int64_t)matrix[3 * i + 1] * raw[1]);
		data_q30[i] += ((int64_t)matrix[3 * i + 2] * raw[2]);
	}
	raw[0] = (int16_t)(data_q30[0] >> 30);
	raw[1] = (int16_t)(data_q30[1] >> 30);
	raw[2] = (int16_t)(data_q30[2] >> 30);
}

void Icm42688::apply_mounting_matrix_32(const int32_t matrix[9], int32_t raw[3])
{
	int64_t data_q30[3];

	for (unsigned i = 0; i < 3; i++)
	{
		data_q30[i] = ((int64_t)matrix[3 * i + 0] * raw[0]);
		data_q30[i] += ((int64_t)matrix[3 * i + 1] * raw[1]);
		data_q30[i] += ((int64_t)matrix[3 * i + 2] * raw[2]);
	}
	raw[0] = (int32_t)(data_q30[0] >> 30);
	raw[1] = (int32_t)(data_q30[1] >> 30);
	raw[2] = (int32_t)(data_q30[2] >> 30);
}

uint32_t Icm42688::odr_bitfield_to_us(uint32_t odr_bitfield)
{
	switch ((ICM426XX_GYRO_CONFIG0_ODR_t)odr_bitfield)
	{
	case ICM426XX_GYRO_CONFIG0_ODR_1_KHZ:
		return 1000;
	case ICM426XX_GYRO_CONFIG0_ODR_500_HZ:
		return 2000;
	case ICM426XX_GYRO_CONFIG0_ODR_200_HZ:
		return 5000;
	case ICM426XX_GYRO_CONFIG0_ODR_100_HZ:
		return 10000;
	case ICM426XX_GYRO_CONFIG0_ODR_50_HZ:
		return 20000;
	default:
		return 640000;
	}
}

#if !IS_HIGH_RES_MODE
int Icm42688::gyro_fsr_dps_to_bitfield(int32_t fsr)
{
	switch (fsr)
	{
#if !defined(ICM42686P) && !defined(ICM42686V)
	case 15:
	case 16:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_16dps;
#endif
	case 31:
	case 32:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_31dps;
	case 62:
	case 63:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_62dps;
	case 125:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_125dps;
	case 250:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_250dps;
	case 500:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_500dps;
	case 1000:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_1000dps;
	case 2000:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_2000dps;
#if defined(ICM42686P) || defined(ICM42686V)
	case 4000:
		return ICM426XX_GYRO_CONFIG0_FS_SEL_4000dps;
#endif
	default:
		return -1;
	}
}

int Icm42688::accel_fsr_g_to_bitfield(int32_t fsr)
{
	switch (fsr)
	{
	case 2:
		return ICM426XX_ACCEL_CONFIG0_FS_SEL_2g;
	case 4:
		return ICM426XX_ACCEL_CONFIG0_FS_SEL_4g;
	case 8:
		return ICM426XX_ACCEL_CONFIG0_FS_SEL_8g;
	case 16:
		return ICM426XX_ACCEL_CONFIG0_FS_SEL_16g;
#if defined(ICM42686P) || defined(ICM42686V)
	case 32:
		return ICM426XX_ACCEL_CONFIG0_FS_SEL_32g;
#endif
	default:
		return -1;
	}
}
#endif


/*********************************设置部分函数****************************************/
void Icm42688::set_acc_sensitivity()
{
	switch (acc_fsr)
	{
	case 2:
		acc_sensitivity = 16384.0f;
		break;
	case 4:
		acc_sensitivity = 8192.0f;
		break;
	case 8:
		acc_sensitivity = 4096.0f;
		break;
	case 16:
		acc_sensitivity = 2048.0f;
		break;

	default:
		break;
	}
}

void Icm42688::set_gyro_snesitivity()
{
	switch (gyr_fsr)
	{
	case 2000:
		gyr_sensitivity = 16.4f;
		break;
	case 1000:
		gyr_sensitivity = 32.8f;
		break;
	case 500:
		gyr_sensitivity = 65.5f;
		break;
	case 250:
		gyr_sensitivity = 131.0f;
		break;
	case 125:
		gyr_sensitivity = 262.0f;
		break;
	case 62:
	case 63:
		gyr_sensitivity = 524.3f;
		break;
	
	case 31:
	case 32:
		gyr_sensitivity = 1048.6f;
		break;

	case 15:
	case 16:
		gyr_sensitivity = 2097.2f;
		break;
	

	default:
		break;
	}
}